%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   本来应该是个函数。输入为grid_map, 返回th1和th2。为了方便调试打成了脚本。不影响。
%   th1和th2分别为自动输出的阈值。th1为dh的分界点，th2为dp的分界点。其中：
%   dh为各格点的最大高度与最小高度之差，分界点为dh排序后求一阶差分的第一个大于0.4的极大值。
%   dp为各格点的最大高度间隔。分界点同样为dp排序后求一阶差分的第一个大于0.4的极大值。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dh = [grid_map(:,:).dh];
ddh = sort(dh);
for i = 1:grid_num
    for j = 1:grid_num
        if isempty(grid_map(i,j).dp)
            dp(i,j) = 0;
        else
            dp(i,j) = max(grid_map(i,j).dp);
        end
        
    end
end
ddp = reshape(dp,[grid_num*grid_num 1]);
ddp = sort(ddp);
d1 = diff(ddh(1:9980));
d2 = diff(ddp(1:9980));
d1 = (d1(:)-min(d1))/(max(d1)-min(d1));
d2 = (d2(:)-min(d2))/(max(d2)-min(d2));
dd1 = d1>0.4;
dd2 = d2>0.4;
ind1 = find(dd1,1,'first');
ind2 = find(dd2,1,'first');
th1 = ddh(ind1);
th2 = ddp(ind2);

% figure;
% bar(d1(9500:9970));
% figure;
% bar(d2(9500:9970));
% figure;
% subplot(2,2,1);
% bar(ddh);
% subplot(2,2,2);
% bar(ddp);
% subplot(2,2,3);
% bar(d1);
% subplot(2,2,4);
% bar(d2);
% d = d1+d2';
% % bar(ddh)
% % figure;
% % bar(ddp)

% [~,ind1] = max(d(1:9980));
% th1 = ddh(ind1);
% %[~,ind2]= max(d2(1:9980));
% th2 = ddp(ind1);
% ma= 0;


